%% 估计杆长(有90度限位，脚可转0-90)
%电机轴是原点
alpha=100; %摆杆轴相对于电机轴角度，可调 90,95,100,105,110
e=65; %两轴距离mm，可调
l1=32; %假定曲柄长mm
l2=48; %假定摆筒长mm
ef1=28; %脚后跟-脚踝距离，假设定值
ef2=50; %前脚掌长（假定）
sita=0:3:360; %曲柄转角
%lr=(min(p+l2+ef1)+max(p+l2-ef1))/2+6=120

p=sqrt(e.*e+l1.*l1-2*e.*l1.*cosd((alpha-sita))); %摆杆上段长度
Cfai=(l1.*cosd(sita)-e.*cosd(alpha))./p;
Sfai=(l1.*sind(sita)-e.*sind(alpha))./p;
fai=atan2(Sfai,Cfai)/pi*180;

x_A=e.*cosd(alpha)+(l2+p).*cosd(fai);
y_A=e.*sind(alpha)+(l2+p).*sind(fai);

% 脚接点
[pl2_max,Num_pl2max]=max(p+l2); %摆杆+筒最长位置
%假设脚面在最低点水平，可算初始所需最长绳长
lr=(min(p+l2+ef1)+max(p+l2-ef1))/2+6;
beta=acosd(((p+l2).*(p+l2)+ef1*ef1-lr*lr)./(2.*(p+l2).*ef1));

beta_f=zeros(1,max(size(sita)));
for j=1:(max(size(sita)))
    if beta(j)<=90
       beta_f(j)=real(beta(j)); 
    else
       beta_f(j)=90; 
    end
end

X_fe=zeros(1,max(size(sita)));Y_fe=zeros(1,max(size(sita)));
for j=1:(max(size(sita)))
       X_fe(j)=x_A(j)-ef2*cosd(180+fai(j)-beta_f(j));
       Y_fe(j)=y_A(j)-ef2*sind(180+fai(j)-beta_f(j));  
end

 IR=zeros(1,max(size(sita)));
for j=1:(max(size(sita)))
     IR(j)=isreal(beta(j));
end

[beta90,sita1num]=min(abs(beta_f-90)); %需要脚悬空点1
sita1=sita(sita1num);
[~,sita2num]=min(abs(sita-(180-acosd(e*sind(alpha-90)/l1)))); %需要脚悬空点2
sita2=sita(sita2num); 
 
%% 画图
%踝关节图像
figure(1)
plot(x_A,y_A,'o'),xlabel('踝关节位置')
hold on
for j=1:max(size(sita))
    if rem(j,3)==0
        c = num2str(j*3);%数字转字符
        text(x_A(j),y_A(j),c);%在图上显示文字
    end
end
scatter(x_A(sita1num:sita2num),y_A(sita1num:sita2num),'^')
hold off

%筛选之后的beta角
figure(2)
plot(sita,beta_f)

%脚点位筛选之后的beta角
figure(3)
plot(X_fe,Y_fe,'o')
hold on
for j=1:max(size(sita))
    if rem(j,3)==0
        c = num2str(j*3);%数字转字符
        text(X_fe(j),Y_fe(j),c);%在图上显示文字
    end
end
scatter(X_fe(sita1num:sita2num),Y_fe(sita1num:sita2num),'^')
hold off

%脚横纵坐标
figure(4)
plot(sita,X_fe,sita,Y_fe), legend('X_fe', 'Y_fe')

figure(5) %绳长应在两条曲线之间取定值
plot(sita,p+l2+ef1,sita,abs(p+l2-ef1))

figure(6)
plot(x_A,y_A,'o')
hold on
plot(X_fe,Y_fe,'^')
xx=[x_A,X_fe];yy=[y_A,Y_fe];
xy_fa=[(xx.'),(yy.')];
xy_Alink=zeros(2*max(size(sita)),2*max(size(sita)));
for j =1:max(size(sita))
    xy_Alink(j,j+max(size(sita)))=1;
    xy_Alink(j+max(size(sita)),j)=1;
end
gplot(xy_Alink,xy_fa,'b')
hold off
% %验算脚长
% Lf=sqrt((X_fe-x_A).^2+(Y_fe-y_A).^2);
% figure(7)
% plot(sita,Lf)